// nxtremote.cpp : main project file.

#include <string>
#include <iostream>
#include <conio.h>
#include <cstdlib> 

#include "nxtcomm-2.0/bluetooth.h"
#include "nxtcomm-2.0/motor.h"
#include "nxtcomm-2.0/error.h"

#include "../KrautMobil/KrautMobil.h"

int main(int argc, char* argv[])
{
   Bluetooth bt;
   
   if ( 2 != argc )
   {
	   std::cerr << "Usage: " << argv[0] << " <port>" << std::endl;
	   std::cerr << "  <port> COM port number to use to connect to NXT" << std::endl;
	   return -1;
   }

   int nxtPort = atoi(argv[1]);
   
   bool nxtConnected = false;
   std::cout << "Waiting for NXT (using port #" << nxtPort << ")" << std::endl;
   while (!nxtConnected)
   {
      try
      {
         bt.connect(nxtPort);
         nxtConnected = true;
      }
      catch(Nxt_exception e)
      {
         nxtConnected = false;
      }
   }

   std::cout << "Connected; run program, then press any of the following keys:" << std::endl;
   std::cout << "'w' to accelerate"       << std::endl;
   std::cout << "'s' to brake"            << std::endl;
   std::cout << "'a' to steer left"       << std::endl;
   std::cout << "'d' to steer right"      << std::endl;
   std::cout << "'t' to calibrate steering"      << std::endl;
   std::cout << "<space> to stop"         << std::endl;
   std::cout << "'q' to quit the program" << std::endl;

   signed char currentSpeed = 0;
   signed char currentAngle = 0;

   int key = getch();
   while('q' != key)
   {
      switch(key)
      {
      case 'a':
         {
            // steer left
            currentAngle -= 10;
            if ( currentAngle < -100 ) currentAngle = -100;
            char msg[] = {
               KRAUT_MSG_START_BYTE,
               KRAUT_CMD_REQ_STEER,
               currentAngle,
               KRAUT_MSG_END_BYTE
            };
            bt.send((unsigned char *)msg, KRAUT_MSG_TOTAL_LEN);
            break;
         }
      case 'd':
         {
            // steer right
            currentAngle += 10;
            if ( currentAngle > 100 ) currentAngle = 100;
            char msg[] = {
               KRAUT_MSG_START_BYTE,
               KRAUT_CMD_REQ_STEER,
               currentAngle,
               KRAUT_MSG_END_BYTE
            };
            bt.send((unsigned char *)msg, KRAUT_MSG_TOTAL_LEN);
            break;
         }
      case 'w':
         {
            // accelerate
            currentSpeed += 10;
            if ( currentSpeed > 100 ) currentSpeed = 100;
            char msg[] = {
               KRAUT_MSG_START_BYTE,
               KRAUT_CMD_REQ_TRQ,
               currentSpeed,
               KRAUT_MSG_END_BYTE
            };
            bt.send((unsigned char *)msg, KRAUT_MSG_TOTAL_LEN);
            break;
         }
      case 's':
         {
            // brake
            currentSpeed -= 10;
            if ( currentSpeed < -100 ) currentSpeed = -100;
            char msg[] = {
               KRAUT_MSG_START_BYTE,
               KRAUT_CMD_REQ_TRQ,
               currentSpeed,
               KRAUT_MSG_END_BYTE
            };
            bt.send((unsigned char *)msg, KRAUT_MSG_TOTAL_LEN);
            break;
         }
      case 't':
         {
            // calibrate steering request
            char msg[] = {
               KRAUT_MSG_START_BYTE,
               KRAUT_CMD_REQ_STEER_CALIBRATE,
               0,
               KRAUT_MSG_END_BYTE
            };
            bt.send((unsigned char *)msg, KRAUT_MSG_TOTAL_LEN);
            break;
         }
      case ' ':
         {
            // immediate stop
            currentSpeed = 0;
            char msg[] = {
               KRAUT_MSG_START_BYTE,
               KRAUT_CMD_REQ_TRQ,
               currentSpeed,
               KRAUT_MSG_END_BYTE
            };
            bt.send((unsigned char *)msg, KRAUT_MSG_TOTAL_LEN);
            break;
         }
      }
      key = getch();
   }

	bt.disconnect();

   return 0;
}